#include "mesh.h"
#include "util.h"
#include <iostream>
template <class T>
pair<T,T> reverse(const pair<T,T>& p) { return make_pair(p.second, p.first); }

vector<normal> mesh::face_vertex_normals(int face_id) {
    int n = count_edges(face_id);
    int offset = faces[offsets[face_id]];

    vector<normal> normals(n);

    for (int i = 0; i < n; ++i) {
        int first, second;
        first = faces[offset + i];
        if (i == 0) second = faces[offset + n - 1];
        else second = faces[offset + i - 1];

        const vertex& origin = vertices[edges[first].first];
        const vertex& v1 = vertices[edges[first].second];
        const vertex& v2 = vertices[edges[second].first];
        normals[i] = ((v1 - origin).cross(v2 - origin)).normalized();
    }
    return normals;
}

vector<normal> mesh::smooth_normals() {
    vector<normal> normals(vertices.size(), normal{.0f,.0f,.0f});
    for (int face_id = 0; face_id < count_faces(); ++face_id) {
        vector<normal> face_normals = face_vertex_normals(face_id);
        for (int edge_id = 0; edge_id < count_edges(face_id); ++edge_id) {
            normals[edges[faces[offsets[face_id] + edge_id]].first] += face_normals[edge_id];
        }
    }
    for_each(normals.begin(), normals.end(), mem_fun_ref(&vertex::normalize));
    return normals;
}

void mesh::render(float t, bool smooth) {
    vertex light1{sin(t*1.2f), sin(t*1.3f), sin(t)}; light1.normalize();
    vertex light2{cos(t*3.2f), sin(t*0.6f), cos(t*2.1f)}; light2.normalize();
    vertex color1{0.5f, 0.6f, 0.7f};
    vertex color2{0.6f, 0.4f, 0.4f};
    if (smooth) {
        vector<normal> normals = smooth_normals();
        for (int face_id = 0; face_id < count_faces(); ++face_id) {
            glBegin(GL_POLYGON);
            for (int edge_id = 0; edge_id < count_edges(face_id); ++edge_id) {
                float c1 = light1.dot(normals[face_vertex_id(face_id, edge_id)]);
                float c2 = light2.dot(normals[face_vertex_id(face_id, edge_id)]);

                glColor3f(c1 * color1 + c2 * color2);
                glVertex3f(face_vertex(face_id, edge_id));
            }
            glEnd();
        }
    } else {
        for (int face_id = 0; face_id < count_faces(); ++face_id) {
            glBegin(GL_POLYGON);
            vector<normal> normals = face_vertex_normals(face_id);
            for (int edge_id = 0; edge_id < count_edges(face_id); ++edge_id) {
                float c1 = light1.dot(normals[edge_id]);
                float c2 = light2.dot(normals[edge_id]);

                glColor3f(c1 * color1 + c2 * color2);
                glColor3f(coords[faces[offsets[face_id] + edge_id]][0], coords[faces[offsets[face_id] + edge_id]][1], 0.1);
                
                glVertex3f(face_vertex(face_id, edge_id));
            }
            glEnd();
        }

    }
}

void mesh::build_adjacency() {
    int num_edges = faces.size();
    adjedges.resize(num_edges);
    vector<int> f1(boost::counting_iterator<int>(0), boost::counting_iterator<int>(num_edges));
    vector<int> f2(f1);
    singular.clear();

    sort(f1.begin(), f1.end(), [&](int a, int b) { return edges[faces[a]] < edges[faces[b]]; } );
    sort(f2.begin(), f2.end(), [&](int a, int b) { return reverse(edges[faces[a]]) < reverse(edges[faces[b]]);} );

    auto e1 = f1.begin();
    auto e2 = f2.begin();
    while ((e1 != f1.end()) and (e2 != f2.end())) {
        edge f_edge = edges[faces[*e1]];
        edge r_edge = reverse(edges[faces[*e2]]);

        if (f_edge == r_edge) {
            if (f_edge.first < f_edge.second) {
                singular.push_back(*e1);
                adjedges[*e1] = *e2;
                adjedges[*e2] = *e1;
            }
            e1++;
            e2++;
        } else if (f_edge < r_edge) {
            singular.push_back(*e1);
            adjedges[*e1] = -1;
            e1++;
        } else {
            e2++;
        }
    }
}

index_array mesh::build_edgepoint_indices() {
    index_array edge_midpoints(faces.size());
    int offset = vertices.size() + count_faces();
    indexed_for(singular.begin(), singular.end(), [&](const int& i, const int& single) {
            edge_midpoints[single] = offset + i;
            if (this->opposite_edge(single) != -1) edge_midpoints[this->opposite_edge(single)] = offset + i;
            });
    return edge_midpoints;
}

mesh mesh::subdivide(void) {
    vector<short int> valencies(vertices.size()); 
    index_array edge_midpoints = build_edgepoint_indices();

    mesh subdiv(vertices.size() + singular.size() + count_faces(), faces.size() * 4, faces.size());

    auto op_start = subdiv.vertices.begin();
    auto op_end = subdiv.vertices.begin() + vertices.size();
    auto fp_start = op_end;
    auto fp_end = subdiv.vertices.begin() + vertices.size() + count_faces();
    auto ep_start = fp_end;
    auto ep_end = subdiv.vertices.end();

    for (size_t i = 0; i < subdiv.offsets.size(); ++i) subdiv.offsets[i] = i * 4;

    indexed_for(offsets.begin(), offsets.end() - 1, offsets.begin() + 1, offsets.end(), [&](int face_id, int face_start, int face_end) { 
            auto edge_start = faces.begin() + face_start;
            auto edge_end = faces.begin() + face_end;
            auto num_edges = distance(edge_start, edge_end);
            int mid_vertex = vertices.size() + face_id;

            vec2f mid_coord;
            indexed_for(edge_start, edge_end, [&](int i, int edge_id) {
                mid_coord += coords[edge_id];
                });
            mid_coord /= float(num_edges);

            for_each(edge_start, edge_end, [&](int edge_id) {
                int padding = edge_id * 4;
                int e1 = edge_id;
                int e2 = this->previous_edge(face_id, edge_id);
                const edge& edge1 = edges[e1];
                //const edge& edge2 = edges[e2];

                edge edge_tmp[] = {
                    edge(edge1.first, edge_midpoints[e1]), 
                    edge(edge_midpoints[e1], mid_vertex), 
                    edge(mid_vertex, edge_midpoints[e2]), 
                    edge(edge_midpoints[e2], edge1.first) 
                };

                int face_tmp[] = {
                    this->next_edge(this->opposite_edge(e1)) * 4 + 3,
                    this->next_edge(e1) * 4 + 2,
                    this->previous_edge(e1) * 4 + 1,
                    this->opposite_edge(e2) * 4,
                };

                vec2f coord_tmp[] = {
                    this->pcoord(e1),
                    (this->pcoord(e1) + this->pcoord(this->next_edge(e1))) * 0.5f,
                    mid_coord,
                    (this->pcoord(e1) + this->pcoord(e2)) * 0.5f
                };

                for (int i = 0; i < 4; ++i) { 
                    subdiv.edges[padding+i] = edge_tmp[i];
                    subdiv.adjedges[padding+i] = face_tmp[i]; 
                    subdiv.faces[padding+i] = padding+i; 
                    subdiv.parents[padding+i] = edge_id; 
                    subdiv.coords[padding+i] = coord_tmp[i];
                }
            });

            vertex fp {0,0,0};
            for_each(edge_start, edge_end, [&](int edge_id) { fp += this->face_vertex(edge_id); });
            fp /= float(num_edges);
            *(fp_start + face_id) = fp;

            for_each(edge_start, edge_end, [&](int edge_id) { 
                    subdiv.singular.push_back(edge_id * 4 + 1);
                    subdiv.vertices[this->face_vertex_id(edge_id)] += fp;
                    valencies[this->face_vertex_id(edge_id)]++;
                    });
    });

    transform(singular.begin(), singular.end(), ep_start, [&](int se) {
            const edge& single = edges[faces[se]];
            vertex edge_sum = vertices[single.first] + vertices[single.second];
            vertex ep = edge_sum + *(fp_start + this->edge_parent(se)) + *(fp_start + this->edge_parent(this->opposite_edge(se)));
            subdiv.vertices[single.first] += edge_sum;
            subdiv.vertices[single.second] += edge_sum;

            subdiv.singular.push_back(se * 4);
            subdiv.singular.push_back(this->opposite_edge(se) * 4);
            return ep /= 4.0f;
            });

    transform(op_start, op_end, vertices.begin(), valencies.begin(), op_start, 
            [](vertex v1, vertex v2, float n) -> vertex { 
                vertex tmp = v1;
                tmp += v2 * (n - 3.0f) * n;
                return tmp /= n*n;
            });

    subdiv.normals = subdiv.smooth_normals();

    return subdiv;
}

void mesh::to_patches(std::stack<primitive_ptr>& primitives) {
    for (int face_id = 0; face_id < count_faces(); ++face_id) {
        if (count_edges(face_id) != 4) continue;
        bicubic_patch* tmp = new bicubic_patch;
        for (int i = 0; i < 4; ++i)
            for (int j = 0; j < 4; ++j) tmp->vertices[i][j] = vertex{0,0,0};

        int c[4];
        for (int i = 0; i < 4; ++i) {
            c[i] = previous_edge(opposite_edge(next_edge(opposite_edge(offsets[face_id]+i))));
        }

        tmp->vertices[0][0] = first_vertex(c[0]);
        tmp->vertices[0][3] = first_vertex(c[1]);
        tmp->vertices[3][3] = first_vertex(c[2]);
        tmp->vertices[3][0] = first_vertex(c[3]);

        tmp->vertices[0][1] = second_vertex(c[0]);
        tmp->vertices[1][0] = first_vertex(previous_edge(c[0]));

        tmp->vertices[1][3] = second_vertex(c[1]);
        tmp->vertices[0][2] = first_vertex(previous_edge(c[1]));

        tmp->vertices[3][2] = second_vertex(c[2]);
        tmp->vertices[2][3] = first_vertex(previous_edge(c[2]));

        tmp->vertices[2][0] = second_vertex(c[3]);
        tmp->vertices[3][1] = first_vertex(previous_edge(c[3]));

        tmp->vertices[1][1] = first_vertex(offsets[face_id]+0);
        tmp->vertices[1][2] = first_vertex(offsets[face_id]+1);
        tmp->vertices[2][2] = first_vertex(offsets[face_id]+2);
        tmp->vertices[2][1] = first_vertex(offsets[face_id]+3);
        
        primitives.push(tmp);
    }
}

inline float angle(const vertex& a, const vertex& b) {
    float f = a.normalized().dot(b.normalized());
    if (f >= 1.0) return 0.0f;
    if (f <= -1.0) return M_PI;
    float r = acos(f);
    return r;
}

inline float angle(const vertex& a, const vertex& b, const vertex& c) {
    return angle(b - a, c - a);
}

uv mesh::get_uv(int edge_id, float u, float v) {
    float out_u = 0.0f;
    float out_v = 0.0f;
    int face_id = edge_parent(edge_id);
    int edge_n = edge_id - offsets[face_id];
    switch (edge_n) {
        case 0:
            out_u = u;
            out_v = v;
            break;
        case 1:
            out_u = v;
            out_v = 1.0f - u;
            break;
        case 2:
            out_u = 1.0f - u;
            out_v = 1.0f - v;
            break;
        case 3:
            out_u = 1.0f - v;
            out_v = u;
    }
    return uv{face_id, vec2f{out_u, out_v}};
}

inline float clamp(float x, float min, float max) { if (x < min) return min; if (x > max) return max; return x; }
inline float bclamp(float x) { return clamp(x, 0.0001f, 0.99999f); }

segment mesh::sline(float& x, float& y, float& line_angle, int& start_edge, float& len) {
    int quad_edges[4];

    int tmp = start_edge;
    for (int i = 0; i < 4; ++i) {
        quad_edges[i] = tmp;
        tmp = next_edge(tmp);
    }

    //with corner uv's midpoint has the same value as one of the vertices and one edge in
    //following calculations gets zero length, fix by clamping before real solution
    //TODO handle corners separately
    vertex midpoint = lerp2(bclamp(y), bclamp(x), 
            face_vertex(quad_edges[0]), face_vertex(quad_edges[1]),
            face_vertex(quad_edges[2]), face_vertex(quad_edges[3]));


    float angles[4];
    for (int i = 0; i < 4; ++i) {
        angles[i] = angle(midpoint, first_vertex(quad_edges[i]), second_vertex(quad_edges[i]));
    }

    float in_angle = line_angle - angle(first_vertex(quad_edges[0]), second_vertex(quad_edges[0]), midpoint);
    in_angle = fmod(in_angle + M_PI, 2.0f * M_PI);

    float angle_start = 0.0f;
    float angle_end = angles[0];
    int out_edge = start_edge;

    vertex ov1, ov2;

    float null_angle = line_angle;

    int null_edge = start_edge - offsets[edge_parent(start_edge)];
    int null_start = start_edge;
    int null_end = previous_edge(start_edge);
    for (int i = 0; i < null_edge; ++i) {
        float a = angle(second_vertex(null_end), first_vertex(null_end), second_vertex(null_start));
        null_angle += M_PI - a;
        null_start = null_end;
        null_end = previous_edge(null_end);
    }

    for (int i = 0; i < 4; ++i) {
        if (in_angle > angle_start and in_angle < angle_end) {
            ov1 = first_vertex(out_edge);
            ov2 = second_vertex(out_edge);
            in_angle -= angle_start;
            break;
        }
        angle_start = angle_end;
        angle_end += angles[(i+1)%4];
        out_edge = next_edge(out_edge);
    }

    float side_angle = angle(ov1, midpoint, ov2);
    float out_angle = M_PI - side_angle - in_angle;

    float out_pos = ((ov1 - midpoint).norm() * sin(in_angle)) / sin(out_angle);
    out_pos /= (ov2 - ov1).norm();
    
    uv uv_start = get_uv(start_edge, x, y);
    uv uv_end = get_uv(out_edge, 0.0f, out_pos);

    vertex a = lerp2(uv_start.uv[1], uv_start.uv[0], face_vertex(uv_start.face, 0), face_vertex(uv_start.face, 1),
            face_vertex(uv_start.face, 2), face_vertex(uv_start.face, 3));
    
    vertex b = lerp2(uv_end.uv[1], uv_end.uv[0], face_vertex(uv_end.face, 0), face_vertex(uv_end.face, 1),
            face_vertex(uv_end.face, 2), face_vertex(uv_end.face, 3));

    x = 0.0f;
    y = 1.0f - out_pos;
    //if (y < 0.000001f) y = 0.000001f;
    //if (y > 0.999999f) y = 0.999999f;
    line_angle = M_PI - out_angle;
    start_edge = opposite_edge(out_edge);
    float norm = (b - a).norm();
    len += norm;

    return segment{uv_start.face, norm, null_angle, uv_start.uv, uv_end.uv};
}

void mesh::init_coords() {
    coords.resize(edges.size());

    vec2f tmp_coords[] = {
        vec2f{0,0}, vec2f{0,1}, vec2f{1,1}, vec2f{1,0}, vec2f{1,0}, vec2f{1,0}
    };

    for (int face_id = 0; face_id < count_faces(); ++face_id) {
        int num_edges = count_edges(face_id);
        if (num_edges < 7) {
            for (int edge_id = 0; edge_id < 4; ++edge_id) {
                coords[faces[offsets[face_id] + edge_id]] = tmp_coords[edge_id];
            }
        }
    }
}

vec2f mesh::get_st(int face_id, const vec2f& uv) {
    int first = offsets[face_id];
    return lerp2(uv[1], uv[0], coords[first+0], coords[first+1], coords[first+2], coords[first+3]);
}

void mesh::write_ptex(float tr) {
    std::string error;
    PtexPtr<PtexWriter> wr(PtexWriter::open("foo.ptx", Ptex::mt_quad, Ptex::dt_float, 4, 3, count_faces(), error));
    if (!wr) {
        std::cerr << error << std::endl;
        return;
    }

    for (int face_id = 0; face_id < count_faces(); ++face_id) {
        //might a a good idea to subdivide once so all faces are quads anyway
        if (count_edges(face_id) == 4) {

            int e1 = offsets[face_id];
            int e2 = offsets[face_id]+1;
            int e3 = offsets[face_id]+2;
            int e4 = offsets[face_id]+3;

            float du = ((first_vertex(e1) - second_vertex(e1)).norm() + (first_vertex(e3) - second_vertex(e3)).norm()) / 2.0f;
            float dv = ((first_vertex(e2) - second_vertex(e2)).norm() + (first_vertex(e4) - second_vertex(e4)).norm()) / 2.0f;

            int U = ceil(log(du * tr) / log(2.0f));
            int V = ceil(log(dv * tr) / log(2.0f));
            U = max(U, 0);
            V = max(V, 0);
            int Uu = 2 << U;
            int Vv = 2 << V;
            if (U == 0) Uu = 1;
            if (V == 0) Vv = 1;

            cout << face_id << " / " << count_faces() << " : " << Uu << " " << Vv << endl;
            

            float *data = new float[Uu*Vv*4];
            for (int i = 0; i < Uu*Vv*4; ++i) data[i] = (rand() / float(RAND_MAX)) * 0.2f + (face_id % 20) / 20.0f;
            int _adjfaces[4]; 
            int _adjedges[4]; 
            for (int i = 0; i < 4; ++i) {
                _adjedges[i] = adjedges[offsets[face_id] + i];
                _adjfaces[i] = parents[_adjedges[i]];
            }
            wr->writeFace(face_id, Ptex::FaceInfo(Ptex::Res(U, V), _adjfaces, _adjedges), data);
            delete data;
        }
    }

    if (!wr->close(error)) {
        std::cerr << error << std::endl;
        return;
    }

}
